#include <ros_koala.h>

#define PI 3.1415926
#define WHEEL_DIAMETER 0.27
#define COUNTS 5850
float ENCODER_SF = PI * WHEEL_DIAMETER / COUNTS;

//(distance*1000*1.8*1.015)/80

Koala::Koala()
    :robot("/dev/ttyUSB0")
{
    initialized = false;
    robot.setWheelAcceleration(1000000,1000000);
    robot.setWheelSpeed((500  - 10*10)/10*1.8,(500  - 10*10)/10*1.8);

    koala_drive_distance = nh.advertiseService("/koala/drive_distance", &Koala::drive_distance, this);
    koala_turn_angle = nh.advertiseService("/koala/turn_angle", &Koala::turn_angle, this);
    koala_get_wheel_speed = nh.advertiseService("/koala/get_wheel_speed", &Koala::get_wheel_speed, this);
    koala_get_wheel_acceleration = nh.advertiseService("/koala/get_wheel_acceleration", &Koala::get_wheel_acceleration, this);
    koala_left_wheel_count = nh.advertise<std_msgs::Int16>("lwheel", 1000);
    koala_right_wheel_count = nh.advertise<std_msgs::Int16>("rwheel", 1000);
    koala_drive_speed = nh.subscribe<geometry_msgs::Twist>("/koala/twist", 1, &Koala::drive_speed, this);//boost::bind(&Koala::drive_speed, this, _1));
    odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);

    koala_turn_angle_sub = nh.subscribe<std_msgs::Int32>("/koala/turn_angle_pub", 1, &Koala::turn_angle_pub, this);
    koala_drive_distance_sub = nh.subscribe<std_msgs::Int32>("/koala/drive_distance_pub", 1, boost::bind(&Koala::drive_distance_pub, this, _1));
}

void Koala::publish_odometry()
{
    /*
    ENCODER_SF = PI * WHEEL_DIAMETER / COUNTS
    DISPLACEMENT = (LEFT_COUNTS + RIGHT_COUNTS) * ENCODER_SF / 2
    ROTATION = (LEFT_COUNTS – RIGHT_COUNTS) * ENCODER_SF / TRACK
    POS_X = POS_X[-1] + DISPLACEMENT * cos(HEADING + ROTATION / 2)
    POS_Y = POS_Y[-1] + DISPLACEMENT * sin(HEADING + ROTATION / 2)
    */
    if (!initialized) {
        current_time  = ros::Time::now();
        last_time = ros::Time::now();
        old_encoder_left =  robot.getLeftWheelCounter();
        old_encoder_right =  robot.getRightWheelCounter();

        lastTrans = 0.0;
        lastRot = 0.0;

        oldx = 0.0;
        oldy = 0.0;
        oldth = 0.0;

        x = 0.0;
        y = 0.0;
        th = 0.0;

        R = 0.0;
        w = 0.0;

        initialized = true;
    }
    else {

        // Update time (s)
        current_time = ros::Time::now();

        // get encoder values & compute delta encoder values
        float new_encoder_left = robot.getLeftWheelCounter();
        float new_encoder_right = robot.getRightWheelCounter();
        float delta_encoder_left = new_encoder_left - old_encoder_left;
        float delta_encoder_right =  new_encoder_right - old_encoder_right;
        old_encoder_left = new_encoder_left;
        old_encoder_right = new_encoder_right;


        // Update distances (mm)
        float trans = (delta_encoder_left + delta_encoder_right) / 2 / 22.8375 * 0.001 ;
        // (rads)
        float dRot = (delta_encoder_right - delta_encoder_left) /2 / 22.8375 * 0.001 / 0.1684;

        //---------------------
        // Rot should be angle change total, not change since last call
        // -------------------


        // Find distance differences (m)
        float dTrans = trans;// - lastTrans;
        lastRot = dRot + lastRot;//rot - lastRot;  //difference in rotation from last call

        //std::cout << "ROTATION: " << lastRot << std::endl;

        //if (dTrans < -2.0 || dTrans > 2.0) return;

        // Find time difference (s)

        float dTime = (current_time - last_time).toSec();

        // Find th, x and y (m)

        if(!dTrans==0)
        {
            float x_moved = cos( dRot ) * dTrans;
            float y_moved = -sin( dRot ) * dTrans;

            // calculate the final position of the robot
            x = x + ( cos( lastRot ) * x_moved - sin( lastRot ) * y_moved );
            y = y + ( sin( lastRot ) * x_moved + cos( lastRot ) * y_moved );
        }
        //std::cout<< x << " " << y << std::endl;
        if(!dRot==0)
        {
            //lastRot = lastRot + dRot;
        }

        float distanceSpeed = dTrans / dTime;
        float rotSpeed = dRot / dTime;


        //since all odometry is 6DOF we'll need a quaternion created from yaw
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(lastRot);

        //first, we'll publish the transform over tf
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";

        odom_trans.transform.translation.x = x;
        odom_trans.transform.translation.y = y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;

        //send the transform
        broadcaster.sendTransform(odom_trans);

        //next, we'll publish the odometry message over ROS
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";

        double rotCovar = 1.0;
        if (dRot == 0) {rotCovar = 0.00000000001;}

        //set the position
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;

        //set the velocity
        odom.child_frame_id = "base_link";
        odom.twist.twist.linear.x = distanceSpeed;
        odom.twist.twist.linear.y = 0;
        odom.twist.twist.angular.z = rotSpeed;

        //publish the message
        odom_pub.publish(odom);
        //lastTrans = trans;
        //lastRot = rot;

        last_time = current_time;
    }

}

void Koala::drive_speed(const geometry_msgs::Twist::ConstPtr& msg)
{
    float rwheel, lwheel;
    float dx = msg->linear.x;
    float dr = msg->angular.z;

    std::cout << "DISTANCE: " << dx << " ANGLE: " << dr << std::endl;

    if(dx!=0)
    {
            rwheel = (500  - dr*10)/10*1.8;
            lwheel = (500  + dr*10)/10*1.8;
    }
    else
    {
        rwheel=0;
        lwheel=0;
    }
    std::cout << "rwheel: " << rwheel << " lwheel: " << lwheel << std::endl;
    robot.moveSpeed(rwheel,lwheel);
}

void Koala::get_left_wheel_counter()
{
    std_msgs::Int16 out;
    int count = robot.getLeftWheelCounter();
    out.data = count;
    koala_left_wheel_count.publish(out);
}

void Koala::get_right_wheel_counter()
{
    std_msgs::Int16 out;
    int count = robot.getRightWheelCounter();
    out.data = count;
    koala_right_wheel_count.publish(out);
}


bool Koala::set_wheel_acceleration(koala_kinematics::Int_Bool::Request &in,
                                   koala_kinematics::Int_Bool::Response &out)
{
    if(robot.setWheelAcceleration(in.req, in.req))
    {
        out.res = true;
        return true;
    }
    out.res = false;
    return false;
}

bool Koala::set_wheel_speed(koala_kinematics::Int_Bool::Request &in,
                            koala_kinematics::Int_Bool::Response &out)
{
    if(robot.setWheelSpeed(in.req, in.req))
    {
        out.res = true;
        return true;
    }
    out.res = false;
    return false;
}

bool Koala::get_wheel_acceleration(koala_kinematics::Empty_Int::Request &in,
                                   koala_kinematics::Empty_Int::Response &out)
{
    out.res = robot.getLeftWheelAcceleration();
    return true;
}

bool Koala::get_wheel_speed(koala_kinematics::Empty_Int::Request &in,
                                   koala_kinematics::Empty_Int::Response &out)
{
    out.res = robot.getLeftWheelSpeed();
    return true;
}

bool Koala::drive_distance(koala_kinematics::Int_Bool::Request &in,
                           koala_kinematics::Int_Bool::Response &out)
{
    if(in.req < 0)
    {
        if(robot.moveBackwardDistance(std::abs(in.req)))
        {
            out.res = true;
            return true;
        }
        else
        {
            out.res = false;
            return false;
        }
    }
    else if(in.req >= 0)
    {
        if(robot.moveForwardDistance(std::abs(in.req)))
        {
            out.res = true;
            return true;
        }
        else
        {
            out.res = false;
            return false;
        }
    }

    out.res = false;
    return false;
}

void Koala::drive_distance_pub(const std_msgs::Int32::ConstPtr &msg)
{
    if(msg->data < 0)
    {
        if(robot.moveBackwardDistance(std::abs(msg->data)))
        {
        }
    }
    else if(msg->data >= 0)
    {
        if(robot.moveForwardDistance(std::abs(msg->data)))
        {
        }
    }
}

bool Koala::turn_angle(koala_kinematics::Int_Bool::Request &in,
                       koala_kinematics::Int_Bool::Response &out)
{
    if(in.req < 0)
    {
        if(robot.turnLeft(std::abs(in.req)))
        {
            out.res = true;
            return true;
        }
        else
        {
            out.res = false;
            return false;
        }
    }
    else if(in.req >= 0)
    {
        if(robot.turnRight(std::abs(in.req)))
        {
            out.res = true;
            return true;
        }
        else
        {
            out.res = false;
            return false;
        }
    }

    out.res = false;
    return false;
}

void Koala::turn_angle_pub(const std_msgs::Int32::ConstPtr &msg)
{
    std::cout << "RECEIVED" <<std::endl;
    if(msg->data < 0)
    {
        robot.turnLeft(std::abs(msg->data));
    }
    else
    {
        robot.turnRight(std::abs(msg->data));
    }
}

